#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>

/// This file copies the odometry from the RTAI and adds a correct header and covariance

nav_msgs::Odometry odom_RTAI;

void odomCallback(const nav_msgs::OdometryConstPtr &msg)
{
   odom_RTAI.pose.pose.position.x = msg->pose.pose.position.x;
   odom_RTAI.pose.pose.position.y = msg->pose.pose.position.y;

   odom_RTAI.pose.pose.orientation.x = msg->pose.pose.orientation.x;
   odom_RTAI.pose.pose.orientation.y = msg->pose.pose.orientation.y;
   odom_RTAI.pose.pose.orientation.z = msg->pose.pose.orientation.z;
   odom_RTAI.pose.pose.orientation.w = msg->pose.pose.orientation.w;

   odom_RTAI.twist.twist.linear.x = msg->twist.twist.linear.x;
   odom_RTAI.twist.twist.linear.y = msg->twist.twist.linear.y;

   //liner.z = angular.z; angular.z is not working throw communication with rtai, so i take linear.z
   odom_RTAI.twist.twist.angular.z = msg->twist.twist.linear.z;
}




int main(int argc, char** argv){

  ros::init(argc, argv, "odom_with_cov");

  ros::NodeHandle n;

  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 50);

  ros::Subscriber odom_sub = n.subscribe("/odom_1", 10, odomCallback);

  odom_RTAI.pose.pose.position.x = 0;
  odom_RTAI.pose.pose.position.y = 0;

  odom_RTAI.pose.pose.orientation.x = 0;
  odom_RTAI.pose.pose.orientation.y = 0;
  odom_RTAI.pose.pose.orientation.z = 0;
  odom_RTAI.pose.pose.orientation.w = 0;

  odom_RTAI.twist.twist.linear.x = 0;
  odom_RTAI.twist.twist.linear.y = 0;

  odom_RTAI.twist.twist.angular.z = 0;

  ros::Rate r(10.0);
  while(n.ok()){
      ros::spinOnce();
      ros::Time current_time = ros::Time::now();

      nav_msgs::Odometry odom;
      odom.header.stamp.sec = current_time.sec;
      odom.header.stamp.nsec = current_time.nsec;
      odom.header.frame_id = "/odom";
      odom.child_frame_id = "/base_footprint";
      odom.pose.pose.position.x = odom_RTAI.pose.pose.position.x;
      odom.pose.pose.position.y = odom_RTAI.pose.pose.position.y;
      odom.pose.pose.position.z = 0.0;

      btQuaternion qt;
      qt.setX(odom_RTAI.pose.pose.orientation.x);
      qt.setY(odom_RTAI.pose.pose.orientation.y);
      qt.setZ(odom_RTAI.pose.pose.orientation.z);
      qt.setW(odom_RTAI.pose.pose.orientation.w);

      ROS_DEBUG( "z_new: %f", qt.getZ() );
      ROS_DEBUG( "z: %f", odom_RTAI.pose.pose.orientation.z );
      ROS_DEBUG( "w_new: %f", qt.getW() );
      ROS_DEBUG( "w: %f", odom_RTAI.pose.pose.orientation.w );

      double roll, pitch, yaw, yaw_cal;

      yaw_cal = 1.0;

      btMatrix3x3(qt).getRPY(roll, pitch, yaw);
      ROS_DEBUG( "RPY = (%lf, %lf, %lf)", roll, pitch, yaw );
      ROS_INFO( "y: %f", yaw * yaw_cal );
      qt.setRPY(0,0,yaw * yaw_cal);

      odom.pose.pose.orientation.x = qt.getX();
      odom.pose.pose.orientation.y = qt.getY();
      odom.pose.pose.orientation.z = qt.getZ();
      odom.pose.pose.orientation.w = qt.getW();

      /*odom.pose.pose.orientation.x = odom_RTAI.pose.pose.orientation.x;
      odom.pose.pose.orientation.y = odom_RTAI.pose.pose.orientation.y;
      odom.pose.pose.orientation.z = odom_RTAI.pose.pose.orientation.z;
      odom.pose.pose.orientation.w = odom_RTAI.pose.pose.orientation.w;*/

      double pose_cov[36] = { 1e-3, 0, 0, 0, 0, 0,
                              0, 1e-3, 0, 0, 0, 0,
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0,
                              0, 0, 0, 0, 1e6, 0,
                              0, 0, 0, 0, 0, 1e3};

      memcpy( &odom.pose.covariance[0], pose_cov, sizeof(double)*36 );
      memcpy( &odom.twist.covariance[0], pose_cov, sizeof(double)*36 );

      odom.twist.twist.linear.x = odom_RTAI.twist.twist.linear.x;
      odom.twist.twist.linear.y = odom_RTAI.twist.twist.linear.y;
      odom.twist.twist.linear.z = 0;

      odom.twist.twist.angular.x = 0.0;
      odom.twist.twist.angular.y = 0.0;
      odom.twist.twist.angular.z = odom_RTAI.twist.twist.angular.z;

      odom_pub.publish( odom );

      r.sleep();
  }
}

